{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Ardumoto Example\n",
    "\n",
    "This example shows how to use the \n",
    "[Ardumoto](https://www.sparkfun.com/products/14180) on the board.\n",
    "Ardumoto supports two DC motor driving.\n",
    "\n",
    "There are also [instructions](https://learn.sparkfun.com/tutorials/ardumoto-shield-kit-hookup-guide) \n",
    "on how to hook up the shield.\n",
    "\n",
    "Motor A and Motor B are connected as below to the Arduino pins:\n",
    "\n",
    "* Default connections\n",
    "\n",
    "| Pin number | Functionality                    |\n",
    "|------------|----------------------------------|\n",
    "| 2          | Direction control for motor A    |\n",
    "| 3          | PWM control (speed) for motor A  |\n",
    "| 4          | Direction control for motor B    |\n",
    "| 11         | PWM control (speed) for motor B  |\n",
    "\n",
    "* Alternate Connections\n",
    "\n",
    "| Pin number | Functionality                    |\n",
    "|------------|----------------------------------|\n",
    "| 8          | Direction control for motor A    |\n",
    "| 9          | PWM control (speed) for motor A  |\n",
    "| 7          | Direction control for motor B    |\n",
    "| 10         | PWM control (speed) for motor B  |\n",
    "       \n",
    "In this notebook, we will assume the first (default) pin configuration.\n",
    "\n",
    "There are multiple ways to hook up the motor to the shield, as shown below:\n",
    "\n",
    "![alt text](data/ardumoto_wiring.jpg)\n",
    "\n",
    "In this notebook, we will assume the second way in the above picture."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/javascript": [
       "\n",
       "require(['notebook/js/codecell'], function(codecell) {\n",
       "  codecell.CodeCell.options_default.highlight_modes[\n",
       "      'magic_text/x-csrc'] = {'reg':[/^%%microblaze/]};\n",
       "  Jupyter.notebook.events.one('kernel_ready.Kernel', function(){\n",
       "      Jupyter.notebook.get_cells().map(function(cell){\n",
       "          if (cell.cell_type == 'code'){ cell.auto_highlight(); } }) ;\n",
       "  });\n",
       "});\n"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from pynq.overlays.base import BaseOverlay\n",
    "\n",
    "base = BaseOverlay(\"base.bit\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. Use Microblaze to control the Ardumoto shield\n",
    "First let's define a few constants."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "MOTOR_A = 0\n",
    "MOTOR_B = 1\n",
    "POLAR_DEFAULT = 0\n",
    "POLAR_REVERSE = 1\n",
    "FORWARD = 0\n",
    "BACKWARD = 1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can use Microblaze program to control the shield."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "%%microblaze base.ARDUINO\n",
    "\n",
    "#include \"xio_switch.h\"\n",
    "#include \"gpio.h\"\n",
    "#include \"timer.h\"\n",
    "\n",
    "#define DEFAULT_PERIOD 625998\n",
    "#define DEFAULT_DUTY 312998\n",
    "\n",
    "#define PWM_A_PIN 3\n",
    "#define PWM_B_PIN 11\n",
    "#define DIR_A_PIN 2\n",
    "#define DIR_B_PIN 4\n",
    "\n",
    "typedef enum motor {\n",
    "MOTOR_A = 0,\n",
    "MOTOR_B = 1,\n",
    "}motor_e;\n",
    "\n",
    "static unsigned int pol_a = 0, pol_b = 0;\n",
    "static unsigned int dir_a = 0, dir_b = 0;\n",
    "static unsigned int duty_a = 50, duty_b = 50;\n",
    "\n",
    "static timer timer_a;\n",
    "static timer timer_b;\n",
    "static gpio gpio_a;\n",
    "static gpio gpio_b;\n",
    "\n",
    "unsigned int init_ardumoto(){    \n",
    "    timer_a = timer_open_device(0);\n",
    "    timer_b = timer_open_device(5);\n",
    "    set_pin(PWM_A_PIN, PWM0);\n",
    "    set_pin(PWM_B_PIN, PWM5);\n",
    "    gpio_a = gpio_open(DIR_A_PIN);\n",
    "    gpio_b = gpio_open(DIR_B_PIN);\n",
    "    gpio_set_direction(gpio_a, GPIO_OUT);\n",
    "    gpio_set_direction(gpio_b, GPIO_OUT);\n",
    "    return 0;\n",
    "}\n",
    "\n",
    "void configure_polar(unsigned int motor, unsigned int polarity){\n",
    "    if (motor == MOTOR_A) {\n",
    "        pol_a = polarity;\n",
    "    }else if (motor == MOTOR_B) {\n",
    "        pol_b = polarity;\n",
    "    }\n",
    "}\n",
    "\n",
    "void set_direction(unsigned int motor, unsigned int direction){\n",
    "    if (motor == MOTOR_A){\n",
    "        dir_a = (direction)? pol_a : !pol_a;\n",
    "    }\n",
    "    else if (motor == MOTOR_B){\n",
    "        dir_b = (direction)? pol_b : !pol_b;\n",
    "    }\n",
    "}\n",
    "\n",
    "void set_speed(unsigned int motor, unsigned int speed){\n",
    "    if (motor == MOTOR_A) {\n",
    "        duty_a = speed;\n",
    "    } else if (motor == MOTOR_B) {\n",
    "        duty_b = speed;\n",
    "    }\n",
    "}\n",
    "\n",
    "void run(unsigned int motor){\n",
    "    if (motor == MOTOR_A) {\n",
    "        gpio_write(gpio_a, dir_a);\n",
    "        timer_pwm_generate(timer_a, DEFAULT_PERIOD, \n",
    "                           duty_a*DEFAULT_PERIOD/100);\n",
    "    }else if(motor == MOTOR_B) {\n",
    "        gpio_write(gpio_b, dir_b);\n",
    "        timer_pwm_generate(timer_b, DEFAULT_PERIOD, \n",
    "                           duty_b*DEFAULT_PERIOD/100);\n",
    "    }\n",
    "}\n",
    "\n",
    "void stop(unsigned int motor){\n",
    "    if (motor == MOTOR_A) {\n",
    "        timer_pwm_stop(timer_a);\n",
    "    }else if (motor == MOTOR_B){\n",
    "        timer_pwm_stop(timer_b);\n",
    "    }\n",
    "}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. Set pin and polarity configurations\n",
    "We have to first initialize the device."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "init_ardumoto()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can then set motor A and B to have the same polarity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "configure_polar(MOTOR_A, POLAR_DEFAULT) \n",
    "configure_polar(MOTOR_B, POLAR_DEFAULT)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Set direction and speed for each motor"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "set_direction(MOTOR_A, FORWARD)\n",
    "set_direction(MOTOR_B, FORWARD)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now let us set motor A speed to be 10% of the maximum speed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "set_speed(MOTOR_A, 10)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Set speed for motor B to be the maximum."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "set_speed(MOTOR_B, 99)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Run each individual motor for a few seconds."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "from time import sleep\n",
    "\n",
    "run(MOTOR_A)\n",
    "sleep(3)\n",
    "stop(MOTOR_A)\n",
    "sleep(1)\n",
    "run(MOTOR_B)\n",
    "sleep(3)\n",
    "stop(MOTOR_B)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4. Run both motors together\n",
    "The following cell will run both motors in the same direction,\n",
    "but with different speeds."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "run(MOTOR_A)\n",
    "run(MOTOR_B)\n",
    "sleep(2)\n",
    "stop(MOTOR_A)\n",
    "stop(MOTOR_B)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Again, the rotation of the motor depends on the wiring to the shield.\n",
    "In our setup, the following cell will result in two motors rotating \n",
    "in opposite directions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "set_direction(MOTOR_A, FORWARD)\n",
    "set_speed(MOTOR_A, 50)\n",
    "\n",
    "set_direction(MOTOR_B, BACKWARD)\n",
    "set_speed(MOTOR_B, 50)\n",
    "\n",
    "run(MOTOR_A)\n",
    "run(MOTOR_B)\n",
    "sleep(3)\n",
    "stop(MOTOR_A)\n",
    "stop(MOTOR_B)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
